from cProfile import label
import math
from pickle import encode_long
from tempfile import tempdir
from turtle import position
import numpy as np
import copy
from Method import *

def Avoid_wall(last_pi, pi, wall, block_list, wall_list, ceilling):
    for i in range(len(wall_list)):
        if wall == wall_list[i]:
            wall_list.pop(i)
            break

    if last_pi == None:
        pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))
        rotated_vector = rotation_2D(pi_vector, -90) #将管道方向向量顺时针旋转90度，用于右绕
        unit_rotated_vector = get_Unit_vector(rotated_vector)
        reverse_rotated_vector = rotation_2D(pi_vector, 90) ##将管道方向向量逆时针旋转90度，用于左绕
        unit_reverse_rotated_vector = get_Unit_vector(reverse_rotated_vector)
        
        move_vector = [unit_rotated_vector, unit_reverse_rotated_vector]
        for j in range(len(move_vector)):
            distance = wall[2] / 10
            move_step = [move_vector[j][i] * distance for i in range(3)]
            start_move_point = pi[0]
            end_move_point = pi[1]
            while(1):
                start_move_point = [start_move_point[i] + move_step[i] for i in range(3)]
                end_move_point = [end_move_point[i] + move_step[i] for i in range(3)]
                new_pi = [start_move_point, end_move_point, pi[2], pi[3], pi[4]]

                distance_vector = [pi[0][i] - start_move_point[i] for i in range(3)]
                move_distance = math.sqrt(sum(e**2 for e in distance_vector))
    
                is_collision = collision_detection(new_pi, wall)
                if is_collision:
                    continue
                
                is_collision1 = False
                for i in range(len(wall_list)):
                    temp = collision_detection(new_pi, wall_list[i]) 
                    if temp:
                        cos_value = get_cos_value(new_pi, wall_list[i])
                        if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                            is_collision1 = True
                            break

                if is_collision1:
                    if j == 1:
                        continue
                    else:
                        break

                if move_distance > 1000:
                    if j == 1:
                        print('移出墙体失败')
                        return [pi, [0, 0, 0], None, pi[1]]
                    else:
                        break

                rectify = [new_pi[0][i] - pi[0][i] for i in range(3)]
                return [new_pi, rectify, None, None]
        
    else:
        lengthen_vector = list(map(lambda x, y: x - y, last_pi[1], last_pi[0]))
        shorten_vector = list(map(lambda x, y: x - y, last_pi[0], last_pi[1]))
        unit_lengthen_vector = get_Unit_vector(lengthen_vector)
        unit_shorten_vector = get_Unit_vector(shorten_vector)
        move_vector = [unit_shorten_vector, unit_lengthen_vector] #先尝试缩短，再尝试延长

        for j in range(len(move_vector)):
            distance = wall[2] / 10
            move_step = [move_vector[j][i] * distance for i in range(3)]
            move_point = last_pi[1]
            while(1):
                move_point = [move_point[i] + move_step[i] for i in range(3)]
                move_rectify = [move_point[i] - last_pi[1][i] for i in range(3)]
                move_distance = math.sqrt(sum(e**2 for e in move_rectify))
                if j == 0:
                    last_pi_vector = list(map(lambda x, y: x - y, last_pi[1], last_pi[0]))
                    backward_max = np.dot(last_pi[0], last_pi_vector) #最大回退点
                    backward_value = np.dot(move_point, last_pi_vector) #回退点
                    if backward_value < backward_max: #若回退点超出最大回退点
                        break
            
                new_pi = [[pi[0][i] + move_rectify[i] for i in range(3)], [pi[1][i] + move_rectify[i] for i in range(3)], pi[2], pi[3], pi[4]] #移动后的管道信息

                is_collision = collision_detection(new_pi, wall)
                if is_collision:
                    continue
                
                is_collision1 = False
                for i in range(len(wall_list)):
                    if collision_detection(new_pi, wall_list[i]):
                        cos_value = get_cos_value(new_pi, wall_list[i])
                        if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                            is_collision1 = True
                            break

                if is_collision1:
                    if j == 0:
                        break
                    else:
                        continue

                if new_pi[0][2] <= ceilling[0] or new_pi[1][2] <= ceilling[0] or new_pi[0][2] >= ceilling[1] or new_pi[1][2] >= ceilling[1]:
                    if j == 0:
                        break
                    else:
                        print('移出墙体失败')
                        return [pi, [0, 0, 0], None, pi[1]]

                if move_distance > 1000:
                    if j == 0:
                        break
                    else:
                        print('移出墙体失败')
                        return [pi, [0, 0, 0], None, pi[1]]
                
                new_last_pi_end = [last_pi[1][i] + move_rectify[i] for i in range(3)]
                new_last_pi = [last_pi[0], new_last_pi_end, last_pi[2], last_pi[3], last_pi[4]]
                x1 = pi[1][0]
                y1 = pi[1][1]
                z1 = pi[1][2]
                x2 = new_pi[1][0]
                y2 = new_pi[1][1]
                z2 = new_pi[1][2]
                rectify = [x2 - x1, y2 - y1, z2 - z1]
                return [new_pi, rectify, new_last_pi, None]

def Avoid_move(last_pi, pi, block, block_list, PipeGroup_Type, BlockBase, wall_list, initial_rectify, ceilling, endpipe_flag, stored_newpi_rectify, stored_position):
    from Avoidance_CURD import Pi_bypass
    print('进入移动避障')
    stored_position2 = None
    recursion_position = None
    if last_pi is None:
        move_vector = [0, 0, -1]
        distance = block[3] / 4
        move_step = [move_vector[i] * distance for i in range(3)]
        start_move_point = pi[0]
        end_move_point = pi[1]
        while(1):
            start_move_point = [start_move_point[i] + move_step[i] for i in range(3)]
            end_move_point = [end_move_point[i] + move_step[i] for i in range(3)]
            new_pi = [start_move_point, end_move_point, pi[2], pi[3], pi[4]]
            
            is_collision = collision_detection(new_pi, block)
            if is_collision:
                continue
            
            is_collision1 = False
            for i in range(len(block_list)):
                temp = collision_detection(new_pi, block_list[i]) 
                if temp:
                    is_collision1 = True
                    break
            if is_collision1:
                continue

            move_distance = pi[0][2] - start_move_point[2]
            if move_distance > 2000:
                break

            rectify = [new_pi[0][i] - pi[0][i] for i in range(3)]
            rectify = [rectify[i] + initial_rectify[i] for i in range(3)]

            if new_pi[0][2] <= ceilling[0] or new_pi[1][2] <= ceilling[0]:
                stored_newpi = copy.deepcopy(new_pi)
                stored_rectify = copy.deepcopy(rectify)
                stored_position2 = new_pi[0]
                break

            return [[[new_pi], rectify], 'Done', None, None, [None, None], None]

        pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))
        rotated_vector = rotation_2D(pi_vector, -90) #将管道方向向量顺时针旋转90度，用于右绕
        unit_rotated_vector = get_Unit_vector(rotated_vector)
        reverse_rotated_vector = rotation_2D(pi_vector, 90) ##将管道方向向量逆时针旋转90度，用于左绕
        unit_reverse_rotated_vector = get_Unit_vector(reverse_rotated_vector)

        move_vector = [unit_rotated_vector, unit_reverse_rotated_vector]
        for j in range(len(move_vector)):
            distance = block[2] / 3
            move_step = [move_vector[j][i] * distance for i in range(3)]
            start_move_point = pi[0]
            end_move_point = pi[1]
            while(1):
                start_move_point = [start_move_point[i] + move_step[i] for i in range(3)]
                end_move_point = [end_move_point[i] + move_step[i] for i in range(3)]
                new_pi = [start_move_point, end_move_point, pi[2], pi[3], pi[4]]

                distance_vector = [pi[0][i] - start_move_point[i] for i in range(3)]
                move_distance = math.sqrt(sum(e**2 for e in distance_vector))
                
                is_collision = collision_detection(new_pi, block)
                if is_collision:
                    continue
                
                is_collision2 = False
                for i in range(len(wall_list)):
                    temp = collision_detection(new_pi, wall_list[i])
                    cos_value = get_cos_value(new_pi, wall_list[i])
                    if temp:
                        if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                            is_collision2 = True
                            break
                            
                for i in range(len(block_list)):
                    temp = collision_detection(new_pi, block_list[i])
                    if temp:
                        is_collision2 = True
                        break
                
                if j == 0 and is_collision2:
                    if move_distance < 500:
                        continue
                    else:
                        break
                
                if move_distance > 2000:
                    if j == 1:
                        if stored_position[0] is None:
                            if stored_position2 is None:
                                return [None, 'Fail', None, None, [None, pi[0]], None]
                            else:
                                return [[[stored_newpi], stored_rectify], 'Done', None, None, [stored_position2, None], None]
                        else:
                            return [[stored_newpi_rectify[0], stored_newpi_rectify[1]], 'Done', None, None, [stored_position[0], None], None]
                    else:
                        break

                for i in range(len(block_list)):
                    temp = collision_detection(new_pi, block_list[i])
                    if temp:
                        block_list_leave = copy.deepcopy(block_list)
                        block_list_leave.append(block)
                        new_pi, recursion_position = Pi_bypass([new_pi], block_list, BlockBase, block_list_leave, PipeGroup_Type, ceilling)
                        break

                if len(new_pi[0]) == 3:
                    rectify = [new_pi[0][i] - pi[0][i] for i in range(3)]
                    rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                    return [[[new_pi], rectify], 'Done', None, None, [None, None], recursion_position]
                else:
                    rectify = [new_pi[-1][0][i] - pi[0][i] for i in range(3)]
                    rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                    return [[new_pi, rectify], 'Done', None, None, [None, None], recursion_position]                    
        
    else:
        lengthen_vector = list(map(lambda x, y: x - y, last_pi[1], last_pi[0]))
        shorten_vector = list(map(lambda x, y: x - y, last_pi[0], last_pi[1]))
        unit_lengthen_vector = get_Unit_vector(lengthen_vector)
        unit_shorten_vector = get_Unit_vector(shorten_vector)
        move_vector = [unit_lengthen_vector, unit_shorten_vector]

        for j in range(len(move_vector)):
            distance = block[2] / 3
            move_step = [move_vector[j][i] * distance for i in range(3)]
            move_point = last_pi[1]
            while(1):
                move_point = [move_point[i] + move_step[i] for i in range(3)]
                move_rectify = [move_point[i] - last_pi[1][i] for i in range(3)]
                move_distance_xy = math.sqrt(move_rectify[0] ** 2 + move_rectify[1] ** 2)
                if j == 1:
                    last_pi_vector = list(map(lambda x, y: x - y, last_pi[1], last_pi[0]))
                    backward_max = np.dot(last_pi[0], last_pi_vector) #最大回退点
                    backward_value = np.dot(move_point, last_pi_vector) #回退点
                    if backward_value < backward_max: #若回退点超出最大回退点
                        if stored_position[0] is None:
                            return [None, 'Fail', None, None, [None, pi[0]], None] #避障失败
                        else:
                            return [[stored_newpi_rectify[0], stored_newpi_rectify[1]], 'Done', None, None, [stored_position[0], None], None]
            
                new_pi = [[pi[0][i] + move_rectify[i] for i in range(3)], [pi[1][i] + move_rectify[i] for i in range(3)], pi[2], pi[3], pi[4]] #移动后的管道信息
                
                is_collision = collision_detection(new_pi, block)
                if is_collision:
                    continue
                
                is_collision2 = False
                for i in range(len(wall_list)):
                    temp = collision_detection(new_pi, wall_list[i])
                    cos_value = get_cos_value(new_pi, wall_list[i])
                    if temp:
                        if 0.9659 <= cos_value <= 1 or -1 <= cos_value <= -0.9659:
                            is_collision2 = True
                            break   
                
                for i in range(len(block_list)):
                    temp = collision_detection(new_pi, block_list[i])
                    if temp:
                        is_collision2 = True
                        break
                
                if j == 0 and is_collision2:
                    if move_distance_xy < 500:
                        continue
                    else:
                        break
                
                new_last_pi_end = [last_pi[1][i] + move_rectify[i] for i in range(3)]
                new_last_pi = [last_pi[0], new_last_pi_end, last_pi[2], last_pi[3], last_pi[4]]
                is_collision3 = collision_detection(new_last_pi, block)
                if is_collision3 and j == 0:
                    break
                
                if new_pi[0][2] <= ceilling[0] or new_pi[1][2] <= ceilling[0] or new_pi[0][2] >= ceilling[1] or new_pi[1][2] >= ceilling[1]:
                    if j == 0:
                        break
                    else:
                        return [None, 'Fail', None, None, [None, pi[0]], None]

                if move_distance_xy > 2000:
                    if stored_position[0] is None:
                        return [None, 'Fail', None, None, [None, pi[0]], None]
                    else:
                        return [[stored_newpi_rectify[0], stored_newpi_rectify[1]], 'Done', None, None, [stored_position[0], None], None]
            
                for i in range(len(block_list)):
                    temp = collision_detection(new_pi, block_list[i])
                    if temp:
                        block_list_leave = copy.deepcopy(block_list)
                        block_list_leave.append(block)
                        new_pi, recursion_position = Pi_bypass([new_last_pi, new_pi], block_list, BlockBase, block_list_leave, PipeGroup_Type, ceilling)
                        new_pi.pop(0)
                        break
                    
                if len(new_pi[0]) == 3:
                    new_pi = [new_pi]
                rectify = [new_pi[-1][1][i] - pi[1][i] for i in range(3)]
                rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                new_last_pi[1] = new_pi[0][0]
                return [[new_pi, rectify], 'Done', None, new_last_pi, [None, None], recursion_position]       

def Avoid_left_right(pi, block, block_list, BlockBase, wall_list, PipeGroup_Type, initial_rectify, ceilling, Avoid_Column_flag): #左右避障
    from Avoidance_CURD import Pi_bypass
    print('进入左右避障')
    recursion_position = None
    intersection_points = get_Intersection_points(pi, block) #获取所有的碰撞坐标
    nearest_point = nearest_intersection_point(pi, intersection_points) #获取所有碰撞点中距离管道起点最短的点并投影到管道的方向向量上

    pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))
    block_vector = list(map(lambda x, y: x - y, block[1], block[0]))
    block_xy_length = math.sqrt(block_vector[0] ** 2 + block_vector[1] ** 2)

    try:
        slope = block_vector[2] / block_xy_length
    except:
        slope = float('inf')
    if slope < 0.3:
        #当管道与障碍物夹角在75°-105°，视为垂直碰撞
        unit_pi_vector = get_Unit_vector_2D(pi_vector)
        unit_block_vector = get_Unit_vector_2D(block_vector)
        dot_result = np.dot(unit_pi_vector[0:2], unit_block_vector[0:2]) 
        
        if -0.258 <= dot_result <= 0.258:
            return [[[pi], initial_rectify], 'Change to up-down', None, None, [None, None], None] #若发生垂直碰撞，转为上下避障

    vector_length = math.sqrt(sum(e**2 for e in pi_vector))
    unit_pi_vector = [pi_vector[i]/vector_length for i in range(3)] #管道单位向量

    rotated_vector = rotation_2D(pi_vector, -90) #将管道方向向量顺时针旋转90度，用于右绕
    unit_rotated_vector = [rotated_vector[i] / vector_length for i in range(3)]
    reverse_rotated_vector = rotation_2D(pi_vector, 90) ##将管道方向向量逆时针旋转90度，用于左绕
    unit_reverse_rotated_vector = [reverse_rotated_vector[i] / vector_length for i in range(3)]
    
    block_8point = block_dian2(block)
    left_points = []
    right_points = []
    for i in range(len(block_8point)):
        vector = list(map(lambda x, y: x - y, block_8point[i], pi[0])) #向量：点减去线的起点
        left_or_right = multi_cross(pi_vector, vector) #若大于0，点在方向向量左边，反之在右边
        if left_or_right[2] > 0:
            left_points.append(block_8point[i]) #在方向向量左边的点集合
        else:
            right_points.append(block_8point[i])#在方向向量右边的点集合
    if left_points == []:
        move_vector = [unit_reverse_rotated_vector, unit_rotated_vector]
    elif right_points == []:
        move_vector = [unit_rotated_vector, unit_reverse_rotated_vector]
    else:
        left_max_distance = distance_point_line(left_points[0], [pi[0], pi[1]]) #左边的点中离直线距离最长的点
        for i in range(1, len(left_points)):
            temp = distance_point_line(left_points[i], [pi[0], pi[1]])
            if temp > left_max_distance:
                left_max_distance = temp

        right_max_distance = distance_point_line(right_points[0], [pi[0], pi[1]]) #右边的点中离直线距离最长的点
        for i in range(1, len(right_points)):
            temp = distance_point_line(right_points[i], [pi[0], pi[1]])
            if temp > right_max_distance:
                right_max_distance = temp
        if right_max_distance >= left_max_distance:
            max_distance = left_max_distance
            move_vector = [unit_reverse_rotated_vector, unit_rotated_vector]
        else:
            max_distance = right_max_distance
            move_vector = [unit_rotated_vector, unit_reverse_rotated_vector]

    for j in range(len(move_vector)):
        change_direction = False
        backward_point = nearest_point #从管道最近的碰撞点开始回退
        while(1):
            if change_direction:
                break
            backward_point = get_Backward_point(backward_point, pi) #获取回退后的点

            vector1 = list(map(lambda x, y: x - y, pi[1], backward_point))
            distance1 = math.sqrt(sum(e**2 for e in vector1)) #回退点到原管道终点的距离

            backward_max = np.dot(pi[0], pi_vector) #最大回退点
            backward_value = np.dot(backward_point, pi_vector) #回退点
            if backward_value < backward_max: #若回退点超出最大回退点
                return [[[pi], initial_rectify], 'Enter translational avoidance', None, None, [None, None], None]
            new_pi1 = [pi[0], backward_point, pi[2], pi[3], None]

            horizontal_point = backward_point
            try:
                distance = max_distance / 3
            except:
                distance = pi[2] / 2
            horizontal_step = [move_vector[j][i] * distance for i in range(3)]
            leftright_distance = 0
            while(1):
                horizontal_point = [horizontal_point[i] + horizontal_step[i] for i in range(3)]
                leftright_distance += distance
                new_pi2_start = copy.deepcopy(new_pi1[1])
                new_pi2 = [new_pi2_start, horizontal_point, pi[2], pi[3], None]

                if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe': #若该段管道为水管，为生成的第二段管道设置斜率
                    len_pi = math.sqrt((new_pi2[1][0] - new_pi2[0][0]) ** 2 + (new_pi2[1][1] - new_pi2[0][1]) ** 2)
                    step = len_pi / 100  # z坐标应该下降的高度
                    new_pi2[1][2] = new_pi2[1][2] - step

                is_collision = collision_detection(new_pi1, block)
                is_collision1 = collision_detection(new_pi2, block)
                if is_collision or is_collision1:
                    break

                is_collision2 = False
                for i in range(len(wall_list)):
                    if collision_detection(new_pi2, wall_list[i]):
                        is_collision2 = True
                if is_collision2: #若生成的第二段管道碰上了本循环的障碍物和与原管道碰撞的wall，则继续回退
                    break
                
                step = [unit_pi_vector[i] * distance1 for i in range(3)]
                new_pi13_start = copy.deepcopy(new_pi2[1])
                new_pi3_end = [horizontal_point[i] + step[i] for i in range(3)]
                new_pi3 = [new_pi13_start, new_pi3_end, pi[2], pi[3], None]

                if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe': #若该段管道为水管，为生成的第三段管道设置斜率
                    len_pi = math.sqrt((new_pi3[1][0] - new_pi3[0][0]) ** 2 + (new_pi3[1][1] - new_pi3[0][1]) ** 2)
                    step = len_pi / 100  # z坐标应该下降的高度
                    new_pi3[1][2] = new_pi3[1][2] - step

                is_collision = collision_detection(new_pi3, block) 
                if is_collision: #如生成的第三段管道与本循环的障碍物发生碰撞，则继续延长第二段管道
                    continue

                if leftright_distance <= pi[2] * 3:
                    continue
                
                final_pi = [new_pi1, new_pi2, new_pi3]
                for i in range(len(final_pi)):
                    for k in range(len(block_list)):
                        temp = collision_detection(final_pi[i], block_list[k]) 
                        if temp:  
                            change_direction = True
                            
                if change_direction:
                    if j == 1:
                        if Avoid_Column_flag:
                            block_list_leave = copy.deepcopy(block_list)
                            block_list_leave.append(block)
                            final_pi, recursion_position = Pi_bypass(final_pi, block_list, BlockBase, block_list_leave, PipeGroup_Type, ceilling)
                        else:
                            return [[[pi], initial_rectify], 'Change to up-down', None, None, [None, None], None]
                    else:
                        break

                rectify = [final_pi[-1][1][0] - pi[1][0], final_pi[-1][1][1] - pi[1][1], final_pi[-1][1][2] - pi[1][2]]
                rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                return [[final_pi, rectify], 'Done', None, None, [None, None], recursion_position]

def Avoid_up_down(pi, block, block_list, BlockBase, wall_list, PipeGroup_Type, initial_rectify, ceilling):
    from Avoidance_CURD import Pi_bypass
    print('进入上下避障')
    recursion_position = None
    intersection_points = get_Intersection_points(pi, block) #获取所有的碰撞坐标
    nearest_point = nearest_intersection_point(pi, intersection_points) #获取所有碰撞点中距离管道起点最短的点并投影到管道的方向向量上
    high = block[3] / 5  # 上升或下降的距离
    change_direction = False
    backward_point = nearest_point #从管道最近的碰撞点开始回退
    while(1):
        if change_direction:
            backward_point = nearest_point
        backward_point = get_Backward_point(backward_point, pi) #获取回退后的点
        pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))
        backward_max = np.dot(pi[0], pi_vector) #最大回退点
        backward_value = np.dot(backward_point, pi_vector) #回退点
        if backward_value < backward_max: #若回退点超出最大回退点
            return [[[pi], initial_rectify], 'Enter translational avoidance', None, None, [None, None], None]

        new_pi1 = [pi[0], backward_point, pi[2], pi[3], None]

        downward_point_z = backward_point[2]
        updown_distance = 0
        while (1):
            if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe' or PipeGroup_Type == 'Change to down': #若管道为水管或风管向上避障超过真天花板，则向下避障
                change_direction = False
                downward_point_z = downward_point_z - high
            else: #若管道为风管，向上或向下避障
                if pi[0][2] > pi[1][2]:
                    downward_point_z = downward_point_z - high
                else:
                    downward_point_z = downward_point_z + high
            updown_distance += high
            downward_point = [new_pi1[1][0], new_pi1[1][1], downward_point_z]

            unit_pi_vector = get_Unit_vector(pi_vector)
            if unit_pi_vector[0] == 0:
                angle = math.pi / 2
            else:
                tan_value = unit_pi_vector[1] / unit_pi_vector[0]
                angle = math.atan(tan_value)
            if 0 < angle <= math.pi/2:
                pi2_angle = angle - math.pi/2
            else:
                pi2_angle = angle + math.pi/2
            new_pi2_start = copy.deepcopy(new_pi1[1])
            new_pi2 = [new_pi2_start, downward_point, pi[2], pi[3], pi2_angle]

            is_collision1 = collision_detection(new_pi2, block) #继续回退
            if is_collision1:
                break

            is_collision2 = False
            for i in range(len(wall_list)):
                if collision_detection(new_pi2, wall_list[i]):
                    is_collision2 = True
            if is_collision2: #若生成的第二段管道碰上了本循环的障碍物和与原管道碰撞的wall，则继续回退
                break
                
            new_pi3_start = copy.deepcopy(new_pi2[1])
            new_pi3 = [new_pi3_start, [pi[1][0], pi[1][1], new_pi2[1][2]], pi[2], pi[3], None]

            if PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe': #若该段管道为水管，为生成的第三段管道设置斜率
                len_pi = math.sqrt((new_pi3[1][0] - new_pi3[0][0]) ** 2 + (new_pi3[1][1] - new_pi3[0][1]) ** 2)
                step = len_pi / 100  # z坐标应该下降的高度
                new_pi3[1][2] = new_pi3[1][2] - step

            is_collision = collision_detection(new_pi3, block) #如生成的第三段管道与本循环的障碍物发生碰撞，则继续延长第二段管道
            if is_collision:
                continue

            if updown_distance <= pi[3] * 2.5: #保证第二段管道长度满足生成fitting的条件
                continue

            final_pi = [new_pi1, new_pi2, new_pi3]
            
            if PipeGroup_Type == 'Duct' or PipeGroup_Type == 'LineDuct':
                if new_pi2[0][2] >= ceilling[1] or new_pi2[1][2] >= ceilling[1]:
                    change_direction = True
                    PipeGroup_Type = 'Change to down'

                for i in range(len(final_pi)):
                    for k in range(len(block_list)):
                        temp = collision_detection(final_pi[i], block_list[k]) 
                        if temp:  
                            change_direction = True
                            PipeGroup_Type = 'Change to down'
                            break
            
                if change_direction:
                    break
            
            elif PipeGroup_Type == 'Pipe' or PipeGroup_Type == 'LinePipe' or PipeGroup_Type == 'Change to down':
                if new_pi2[0][2] <= ceilling[0] or new_pi2[1][2] <= ceilling[0]:
                    rectify = [final_pi[-1][1][0] - pi[1][0], final_pi[-1][1][1] - pi[1][1], final_pi[-1][1][2] - pi[1][2]]
                    rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
                    return [[[pi], initial_rectify], 'Enter translational avoidance', [final_pi, rectify], None, [new_pi2[0], None], None]
                
                is_collision3 = False
                for i in range(len(final_pi)):
                    for k in range(len(block_list)):
                        temp = collision_detection(final_pi[i], block_list[k]) 
                        if temp:  
                            is_collision3 = True
                            break
                
                if is_collision3:
                    block_list_leave = copy.deepcopy(block_list)
                    block_list_leave.append(block)
                    change_flag, final_pi, recursion_position = Pi_bypass(final_pi, block_list, BlockBase, block_list_leave, PipeGroup_Type, ceilling) #若生成的三段管道与其他障碍物发生碰撞，则继续调用避障函数
        
            rectify = [final_pi[-1][1][0] - pi[1][0], final_pi[-1][1][1] - pi[1][1], final_pi[-1][1][2] - pi[1][2]]
            rectify = [rectify[i] + initial_rectify[i] for i in range(3)]
            return [[final_pi, rectify], 'Done', None, None, [None, None], recursion_position]

def pipe_preprocess(pi, block, cos_value, group_type):  #若管道与障碍物夹角较小
    pipe_vector = [pi[1][i] - pi[0][i] for i in range(3)]
    pipe_length = math.sqrt(sum(e**2 for e in pipe_vector))
    block_vector = [block[1][i] - block[0][i] for i in range(3)]
    unit_block_vector = get_Unit_vector(block_vector)
    step = [unit_block_vector[i] * pipe_length for i in range(3)]
    if 0.966 < cos_value < 1:
        pi_end = [pi[0][i] + step[i] for i in range(3)]
    else:
        pi_end = [pi[0][i] - step[i] for i in range(3)]
    if group_type == 'Pipe' or group_type == 'LinePipe': #若该段管道为水管，设置斜率
        if pi_end[2] >= pi[0][2]:
            step = pipe_length / 100  #z坐标应该下降的高度
            pi_end[2] = pi_end[2] - step
    new_pi = [pi[0], pi_end, pi[2], pi[3], None]
    x1 = pi[1][0]
    y1 = pi[1][1]
    z1 = pi[1][2]
    x2 = new_pi[1][0]
    y2 = new_pi[1][1]
    z2 = new_pi[1][2]
    rectify = [x2 - x1, y2 - y1, z2 - z1]
    return [[[new_pi], rectify], 'Done', None, None, [None, None], None]


def Avoid_vertical(pi, block, block_list, BlockBase, PipeGroup_Type, initial_rectify, ceilling):
    from Avoidance_CURD import Pi_bypass

    recursion_position = None
    angle = pi[4]
    if angle is None:
        vector1 = [1, 0, 0]
    else:
        vector1 = [1, math.tan(angle), 0]
    vector1 = get_Unit_vector(vector1)
    vector2 = rotation_2D(vector1, 180)
    vector3 = rotation_2D(vector1, 90)
    vector4 = rotation_2D(vector1, -90)
    vector= [vector1, vector2, vector3, vector4]
    intersection_points = get_Intersection_points(pi, block)
    nearest_point = nearest_intersection_point(pi, intersection_points)
    for j in range(len(vector)):
        change_direction = False
        backward_point = nearest_point #从管道最近的碰撞点开始回退
        while(1):
            if change_direction:
                break
            backward_point = get_Backward_point_vertical(backward_point, pi)
            pi_vector = list(map(lambda x, y: x - y, pi[1], pi[0]))
            backward_max = np.dot(pi[0], pi_vector) #最大回退点
            backward_value = np.dot(backward_point, pi_vector) #回退点
            if backward_value < backward_max: #若回退点超出最大回退点，则返回原管道
                return [[[pi], [0, 0, 0]], 'Enter translational avoidance', None, None, [None, None], None]
            new_pi1 = [pi[0], backward_point, pi[2], pi[3], pi[4]]

            horizontal_point = backward_point
            distance = block[2] / 3
            horizontal_step = [vector[j][i] * distance for i in range(3)]
            
            while (1):
                horizontal_point = [horizontal_point[i] + horizontal_step[i] for i in range(3)]
                if j == 0 or j == 1:
                    new_pi2 = [new_pi1[1], horizontal_point, pi[3], pi[2], None]
                else:
                    new_pi2 = [new_pi1[1], horizontal_point, pi[2], pi[3], None]

                if PipeGroup_Type == 'Pipe': #若该段管道为水管，为生成的第二段管道设置斜率
                    len_pi = math.sqrt((new_pi2[1][0] - new_pi2[0][0]) ** 2 + (new_pi2[1][1] - new_pi2[0][1]) ** 2)
                    step = len_pi / 100  # z坐标应该下降的高度
                    new_pi2[1][2] = new_pi2[1][2] - step

                is_collision = collision_detection(new_pi2, block)
                if is_collision:
                    break

                new_pi3 = [new_pi2[1], [new_pi2[1][0], new_pi2[1][1], pi[1][2]], pi[2], pi[3], pi[4]]

                is_collision = collision_detection(new_pi3, block) #如生成的第三段管道与本循环的障碍物发生碰撞，则继续延长第二段管道
                if is_collision:
                    continue
                
                final_pi = [new_pi1, new_pi2, new_pi3]
        
                for i in range(len(final_pi)):
                    for k in range(len(block_list)):
                        temp = collision_detection(final_pi[i], block_list[k]) 
                        if temp:  
                            change_direction = True
                if change_direction:
                    if j == 3:
                        block_list_leave = copy.deepcopy(block_list)
                        block_list_leave.append(block)
                        final_pi, recursion_position = Pi_bypass(final_pi, block_list, BlockBase, block_list_leave, PipeGroup_Type, ceilling) #若生成的三段管道与其他障碍物发生碰撞，则继续调用避障函数
                    else:
                        break 

                rectify = [final_pi[-1][1][0] - pi[1][0], final_pi[-1][1][1] - pi[1][1], final_pi[-1][1][2] - pi[1][2]]
                rectify = [rectify[i] + initial_rectify[i] for i in range(3)]

                return [[final_pi, rectify], 'Done', None, None, [None, None], recursion_position]
